﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

/**
   *  通用数据类型转换
   *  作用：通用数据类型转换
   *  创建时间：2019/07/20 wj
   *  修改时间：2019/08/12 wj
   */
namespace Common.Service
{
    using Common.Define;

    public class Conversion
    {
        private static double INVALID_DATA = 0.000001;

        /**
          * \brief 角度转弧度
          * 
          * \param data double类型输入数值
          * \return 输出结果
          */
        public static double InvaidToZero(double data)
        {
            if (Math.Abs(data) < INVALID_DATA)
            {
                data = 0;
            }
            return data;
        }

        /**
           * \brief 角度转弧度
           *  
           * \param[in] joint 角度数组
           * \retval jointData 转换后弧度数组
           */
        public static double DegreeToRad(double joint)
        {
            return joint * Math.PI / 180.0;
        }

        /**
          * \brief 角度转弧度
          * 
          * \param[in] joint 角度数组
          * \return jointData 转换后弧度数组
          */
        public static double[] DegreeToRads(double[] joint)
        {
            for (uint i = 0; i < joint.Length; i++)
            {
                joint[i] = joint[i] * Math.PI / 180.0;
            }
            return joint;
        }

        /**
          * \brief 弧度转角度
          *  
          * \param[in] joint 弧度
          * \retval jointData 转换后角度
          */
        public static double RadToDegree(double joint)
        {
            return joint * 180.0 / Math.PI;
        }

        /**
         * \brief 数组转世界坐标系参数
         *  
         * \param[in] sum 数组
         * \retval pos 世界坐标系参数
         */
        public static PositionData NumGroupToPosition(double[] sum)
        {
            PositionData pos = new PositionData()
            {
                nx = 0, ny = 0, nz = 0,
                ox = 0, oy = 0, oz = 0,
                ax = 0, ay = 0,  az = 0,
                px = 0, py = 0, pz = 0,
                ox5 = 0, oy5 = 0, oz5 = 0
            };
            pos.nx = InvaidToZero(sum[0]);
            pos.ny = InvaidToZero(sum[4]);
            pos.nz = InvaidToZero(sum[8]);
            pos.ox = InvaidToZero(sum[1]);
            pos.oy = InvaidToZero(sum[5]);
            pos.oz = InvaidToZero(sum[9]);
            pos.ax = InvaidToZero(sum[2]);
            pos.ay = InvaidToZero(sum[6]);
            pos.az = InvaidToZero(sum[10]);
            pos.px = InvaidToZero(sum[3]);
            pos.py = InvaidToZero(sum[7]); 
            pos.pz = InvaidToZero(sum[11]);
            return pos;
        }

        
      /**
         * \brief 机器人辅助坐标系转世界坐标值
         * 
         * \param[in] pos 世界坐标系参数
         * \param[in] pipper 是否满足Piepper准则
         * \retval world 笛卡尔坐标系参数
         */
      public static WorldData PositionToWorld(PositionData  pos, bool pipper) 
      {
          WorldData world = new WorldData();
          world.rz = Math.Atan2(-pos.nz, Math.Sqrt(pos.nx * pos.nx + pos.ny * pos.ny));
          if (1- INVALID_DATA < Math.Abs(Math.Cos(world.rz))) 
          {
              world.ry = InvaidToZero(Math.Atan2(pos.ny, pos.nx) * 180 / Math.PI);
              world.rx = InvaidToZero(Math.Atan2(pos.oz, pos.az) * 180 / Math.PI);
          } 
          else 
          {
              if (1 - INVALID_DATA < Math.Abs(Math.Sin(world.rz))) 
              {
                  world.ry = 0;
                  world.rx = InvaidToZero(Math.Atan2(pos.ox, pos.oy) * 180 / Math.PI);
              } 
              else 
              {
                  world.ry = 0;
                  world.rx = InvaidToZero(Math.Atan2(pos.ox, pos.oy) * 180 / Math.PI);
               }
          }
           world.rz = InvaidToZero(world.rz * 180 / Math.PI);

           world.x = InvaidToZero(pos.px);
           world.y = InvaidToZero(pos.py);
           world.z = InvaidToZero(pos.pz);
           if (pipper)
           {
                world.ox5 = InvaidToZero(pos.ox5);
                world.oy5 = InvaidToZero(pos.oy5);
                world.oz5 = InvaidToZero(pos.oz5);
           }
           return world;
       }

      /**
         * \brief 机器人世界坐标值转辅助坐标系
         * 
         * \param[in] world 笛卡尔坐标系参数
         * \param[in] pipper 是否满足Piepper准则
         * \retval  pos 世界坐标系参数
         */
        public static PositionData WorldToPosition(WorldData world,  bool pipper)
        {
            PositionData pos = new  PositionData(); 
            world.rx = world.rx * Math.PI / 180;
            world.ry = world.ry * Math.PI / 180;
            world.rz = world.rz * Math.PI / 180;
            pos.nx = Math.Cos(world.ry) * Math.Cos(world.rz);
            pos.ny = Math.Sin(world.ry) * Math.Cos(world.rz);
            pos.nz = -Math.Sin(world.rz);
            pos.ox = Math.Cos(world.ry) * Math.Sin(world.rz) * Math.Sin(world.rx) - Math.Sin(world.ry) * Math.Cos(world.rx);
            pos.oy = Math.Sin(world.ry) * Math.Sin(world.rz) * Math.Sin(world.rx) + Math.Cos(world.ry) * Math.Cos(world.rx);
            pos.oz = Math.Cos(world.rz) * Math.Sin(world.rx);
            pos.ax = Math.Cos(world.ry) * Math.Sin(world.rz) * Math.Cos(world.rx) + Math.Sin(world.rx) * Math.Sin(world.ry);
            pos.ay = Math.Sin(world.ry) * Math.Sin(world.rz) * Math.Cos(world.rx) - Math.Sin(world.rx) * Math.Cos(world.ry);
            pos.az = Math.Cos(world.rx) * Math.Cos(world.rz);
            pos.px = world.x;
            pos.py = world.y;
            pos.pz = world.z;
            pos.ox5 = Math.Cos(world.rx) * Math.Cos(world.ry);
            pos.oy5 = Math.Sin(world.rx) * Math.Cos(world.ry);
            pos.oy5 = Math.Sin(world.rx) * Math.Cos(world.ry);
            pos.oy5 = Math.Sin(world.rx) * Math.Cos(world.ry);
            pos.oy5 = Math.Sin(world.rx) * Math.Cos(world.ry);
            pos.oz5 = Math.Cos(world.ry);
            if (pipper) {
                pos.ox5 = world.ox5;
                pos.oy5 = world.oy5;
                pos.oz5 = world.oz5;
            }
            return pos;
        }

    }
}
